#pragma once

/*
 * File - ros_depth_publisher.h
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

#include "ros_video_publishers_base.h"

#include "DepthStreamExt.h"

/**
 * \cond INTERNAL
 */

#include "config.h"
#include <opencv2/opencv.hpp>

#include "depth2pc.h"
/**
 * \endcond
 */

/**
 * \file ros_depth_publisher.h
 *
 * \brief Depth publisher
 */

namespace __INUROS__NAMESPACE__
{
    /**
     * \brief ROS depth publisher
     */
    class CRosDepthPublisher : public CRosVideoPublishersBase
    {
        image_transport::Publisher publisher;
        ros::Publisher pcl_pub;
        void FrameCallback(std::shared_ptr<InuDev::CDepthStream> iStream, std::shared_ptr<const InuDev::CImageFrame> iFrame, InuDev::CInuError iError);
        InuDev::CInuError RegisterCallback();
        InuDev::CInuError UnregisterCallback();

    protected:

        /**
         * \brief StartStream
         *
         * Start depth stream
         */
        virtual InuDev::CInuError StartStream() override;
    virtual InuDev::CInuError InitStream() override;
    public:

        /**
         * \brief Get number of subscribers for publisher
         *
         * Returns number of subscibers to ROS messages this publisher publishes.
         */
        virtual int GetNumSubscribers() override;

        /**
         * \brief ROS Depth Publisher
         */
        CRosDepthPublisher(ros::NodeHandle& _node,  std::shared_ptr<image_transport::ImageTransport> _image_transport, CRosSensor* _rosSensor, std::string iTopicName = "camera/aligned_depth_to_color/image_raw");
    private:

        Inuchip::COptData mOD;
        void LoadOD();
        void pub_pcl(const cv::Mat &dep, uint64_t curTs,int fidx,bool bOrder,int rinter,int cinter);

        int settingConfidence( int newconfidence, bool direct);

        std::shared_ptr<InuDev::CCalibrationData> mCalibrationData;
        
        // Channel recieved from InuRosParams: -1 is default
        int mParamsChannelID;

        // Output format flag: 0 is default (depth)
        int mOutputFormat;
	
	// Post processing flag: 0 is none
        int mPostProcessing;
        
        // Registration Channel: - -1 is default; 
        int mRegistrationChannel;

        // Hole Filter: - 0 is default
        bool mHoleFilterROS;
        
        // Hole Filter: - 0 is default
        bool mHoleFilterSDK;

        // Outlier removal filter - 0 is default
        bool mOutlierRemoveROS;
        
        // Outlier removal filter - 0 is default
        bool mOutlierRemoveSDK;

        // Temperal Filter - 0 is default
        bool mTemporalFilterROS;
        
        // Temperal Filter - 0 is default
        bool mTemporalFilterSDK;
        
        // Passthrough Filter - 0 is default
        bool mPassthroughFilterROS;
        
        // Passthrough Filter - 0 is default
        bool mPassthroughFilterSDK;

        // Show Color - 0 is default
        bool mShowColor;

        // opticalCenter,FocalLength
        float x0,y0,fx,fy;

        /**
         * \brief mDepthFrameID - Used for multiple sensors
         */
        std::string mDepthFrameID;

        int mDepthconfidence;
    };
}
